#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "com.h"
static int imu_fd = 0;

int imu_init(const char* port_name)
{
	imu_fd = uart_open_no_block(port_name);
	printf("bug:%d\n",imu_fd);
	if(imu_fd != 0)
	{
		int err = uart_set(imu_fd, 115200, 0, 8, 1, 'N');
		if (0 == err)
		{
			printf("Open IMU Failed !\n");
			return 0;
		}
		else
		{
			printf("Start Open COM!\n");
			return 1;
		}
	}
}

int get_imu_data(char gps_str[11])
{
	static int i = 0;
	char single_char = 0;
	if (uart_recv_no_block(imu_fd, &single_char, 1) > 0)
	{
		if(single_char==0x55)
		{
			if(i==11)
			{
				//接收到完整的以个
				i = 0;
				return 1;
			}
			else
			{
				i = 0;
				gps_str[i] = single_char;
				i++;
			}
		}
		if(i==1&&single_char==0x53)
		{
			gps_str[i] = single_char;
			i++;
		}
		else if(i>=2 && i<=10)
		{
			gps_str[i] = single_char;
			i++;
		}
	}
	return 0;
}
static float imu_heading = 0.0;
static int imu_update_flag = 0;
int get_imu_heading(float* angle)
{
	if(imu_update_flag==1)
	{
		if(imu_heading < 0)
		{
			*angle = -imu_heading;
		}
		else
		{
			*angle = 360.0 - imu_heading;
		}
		return 1;
	}
	else
	{
		return 0;
	}
}
static char imu_str[11];
int imu_update()
{
	if(imu_fd == 0)
	{
		//printf("imu_heading:%f\n",imu_heading);
		return -1;
	}
	else
	{
		if(get_imu_data(imu_str)==1)
		{
			short data[4];
			memcpy(data, imu_str+2, 8);
			imu_heading = (float)data[2]/32768.0*180.0;
			imu_update_flag = 1;
			printf("imu_heading:%f\n",imu_heading);
			return 1;
		}
		else
		{
			return 0;
		}
	}
}
int imu_main(int argc, char *argv[])
{
	imu_init("/dev/ttyUSB1");
	char imu_str[11];
	while(1)
	{
		if(get_imu_data(imu_str)==1)
		{
			for (int i = 0; i < 11; ++i) 
			{
				printf("%2x", imu_str[i]);			
			}
			printf("\n");

			short data[4];
			memcpy(data, imu_str+2, 8);
			for (int i = 0; i < 3; ++i) 
			{
				printf("%d %d\n",i,data[i]);
			}
			for (int i = 0; i < 3; ++i) 
			{
				printf("%d %f\n",i,(float)data[i]/32768.0*180.0);
			}
		}
	}
	return 0;
}
